
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       ahrs_view.c
  * @author     baiyang
  * @date       2021-9-3
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "ahrs_view.h"

#include <rtconfig.h>

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>

#if CONFIG_HAL_BOARD == HAL_BOARD_RTTHREAD
#include <sensor_imu/sensor_imu.h>
#endif

#include <logger/ap_log.h>
#include <gcs_mavlink/gcs.h>
#include <common/time/gp_time.h>
#include <common/geo/declination.h>
#include <common/console/console.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void update_location_and_state();
static void publish_vehicle_home();
static void update_accel_vibration_metrics(ahrs_view *ahrs);
static void update_gyro_vibration_metrics(ahrs_view *ahrs);
/*----------------------------------variable----------------------------------*/
static ahrs_view ahrs;

static uitc_vehicle_origin origin_pos_g;
static uitc_vehicle_attitude vehicle_attitude;
static uitc_sensor_gyr sensor_gyr;
static uitc_sensor_acc sensor_acc;
static uitc_sensor_mag sensor_mag;
static uitc_sensor_baro sensor_baro;
static uitc_vehicle_local_position local_position;
static uitc_sensor_gps vehicle_gps_position;
static uitc_sensor_airspeed sensor_arspd;
static uitc_estimator_sensor_bias sensor_bias;

static uitc_vehicle_home vehicle_home;

static Location curr_loc_tmp;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
ahrs_view* get_ahrs_view()
{
    return &ahrs;
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void ahrs_view_init()
{
    ahrs.timestamp_us = time_micros64();
    quat_init(&(ahrs.attitude_body));
}

/**
  * @brief       
  * @param[in]   ahrs  
  * @param[out]  
  * @retval      
  * @note        
  */
void ahrs_view_update()
{
    ahrs.timestamp_us = time_micros64();

    itc_copy_from_hub(ITC_ID(vehicle_attitude), &vehicle_attitude);
    itc_copy_from_hub(ITC_ID(sensor_gyr), &sensor_gyr);
    itc_copy_from_hub(ITC_ID(sensor_acc), &sensor_acc);
    itc_copy_from_hub(ITC_ID(sensor_mag), &sensor_mag);
    itc_copy_from_hub(ITC_ID(sensor_baro), &sensor_baro);
    itc_copy_from_hub(ITC_ID(vehicle_local_position), &local_position);
    itc_copy_from_hub(ITC_ID(sensor_gps),  &vehicle_gps_position);
    itc_copy_from_hub(ITC_ID(sensor_airspeed), &sensor_arspd);
    itc_copy_from_hub(ITC_ID(estimator_sensor_bias), &sensor_bias);

    ahrs.roll_sensor_cd = Rad2Deg(vehicle_attitude.vehicle_euler.roll * 100);
    ahrs.pitch_sensor_cd = Rad2Deg(vehicle_attitude.vehicle_euler.pitch * 100);
    ahrs.yaw_sensor_cd = Rad2Deg(vehicle_attitude.vehicle_euler.yaw * 100);

    ahrs.cos_roll = cosf(vehicle_attitude.vehicle_euler.roll);
    ahrs.cos_pitch = cosf(vehicle_attitude.vehicle_euler.pitch);
    ahrs.cos_yaw = cosf(vehicle_attitude.vehicle_euler.yaw);

    ahrs.sin_roll = sinf(vehicle_attitude.vehicle_euler.roll);
    ahrs.sin_pitch = sinf(vehicle_attitude.vehicle_euler.pitch);
    ahrs.sin_yaw = sinf(vehicle_attitude.vehicle_euler.yaw);

    ahrs.attitude_body = vehicle_attitude.vehicle_quat;

    ahrs.roll = vehicle_attitude.vehicle_euler.roll;
    ahrs.pitch = vehicle_attitude.vehicle_euler.pitch;
    ahrs.yaw = vehicle_attitude.vehicle_euler.yaw;

    if (sensor_bias.gyro_bias_valid) {
        ahrs.gyro_bias.x = -sensor_bias.gyro_bias[0];
        ahrs.gyro_bias.y = -sensor_bias.gyro_bias[1];
        ahrs.gyro_bias.z = -sensor_bias.gyro_bias[2];
    }

    ahrs.gyr.x = sensor_gyr.sensor_gyr_filter[0] + ahrs.gyro_bias.x;
    ahrs.gyr.y = sensor_gyr.sensor_gyr_filter[1] + ahrs.gyro_bias.y;
    ahrs.gyr.z = sensor_gyr.sensor_gyr_filter[2] + ahrs.gyro_bias.z;

    if (sensor_bias.accel_bias_valid) {
        ahrs.accel_bias.x = -sensor_bias.accel_bias[0];
        ahrs.accel_bias.y = -sensor_bias.accel_bias[1];
        ahrs.accel_bias.z = -sensor_bias.accel_bias[2];
    }

    ahrs.acc.x = sensor_acc.sensor_acc_filter[0] + ahrs.accel_bias.x;
    ahrs.acc.y = sensor_acc.sensor_acc_filter[1] + ahrs.accel_bias.y;
    ahrs.acc.z = sensor_acc.sensor_acc_filter[2] + ahrs.accel_bias.z;

    quat_vec_rotate(&ahrs.attitude_body, &ahrs.accel_ef, &ahrs.acc);

    update_location_and_state();

    ahrs.relpos_cm.x = local_position.x*100.0f;
    ahrs.relpos_cm.y = local_position.y*100.0f;
    ahrs.relpos_cm.z = -local_position.z * 100.0f;

    ahrs.velocity_cm.x = local_position.vx*100.0f;
    ahrs.velocity_cm.y = local_position.vy*100.0f;
    ahrs.velocity_cm.z = -local_position.vz * 100.0f;

    update_accel_vibration_metrics(&ahrs);
    update_gyro_vibration_metrics(&ahrs);

#if CONFIG_HAL_BOARD == HAL_BOARD_RTTHREAD
    ahrs.ins_is_still = sensor_imu_is_still();
    ahrs.accel_peak_hold_neg_x = sensor_imu_get_singleton()->_peak_hold_state.accel_peak_hold_neg_x;
#else
    #define ACCEL_VIBE_METRIC_MAX  1.2f  // accel_vibration_metric * dt * 2.1e2f > is_moving_scaler
    ahrs.ins_is_still = (ahrs.accel_vibration_metric < ACCEL_VIBE_METRIC_MAX);
#endif

    ahrs.gps_ground_speed = vehicle_gps_position.vel_m_s;
    ahrs.EAS2TAS          = math_rsqrtf(sensor_baro.air_density_ratio);

    ahrs.valid_airspeed      = sensor_arspd.healthy;
    ahrs.equivalent_airspeed = sensor_arspd.airspeed;
    ahrs.true_airspeed       = sensor_arspd.airspeed * ahrs.EAS2TAS;
}

bool ahrs_set_home(ahrs_view *ahrs, const Location *loc)
{
    // check location is valid
    if (loc->lat == 0 && loc->lng == 0 && loc->alt == 0) {
        return false;
    }
    if (!location_check_lat_lng(loc)) {
        return false;
    }
    // home must always be global frame at the moment as .alt is
    // accessed directly by the vehicles and they may not be rigorous
    // in checking the frame type.
    Location tmp = *loc;
    if (!location_change_alt_frame(&tmp, ALT_FRAME_ABSOLUTE)) {
        return false;
    }

    if (!ahrs->home_is_set) {
        // record home is set
        aplog_write_event(LOG_EVENT_SET_HOME);
    }

    ahrs->home_loc = tmp;
    ahrs->home_is_set = true;

    publish_vehicle_home();

    ahrs_log_write_home_and_origin(ahrs);

    // send new home and ekf origin to GCS
    gcs_send_message(MAV_MSG_HOME);
    gcs_send_message(MAV_MSG_ORIGIN);

    return true;
}

// log ahrs home and EKF origin
void ahrs_log_write_home_and_origin(ahrs_view *ahrs)
{
    Location ekf_orig;
    if (ahrs_get_origin(ahrs, &ekf_orig)) {
        aplog_write_origin(APLOG_EKF_ORIGIN, &ekf_orig);
    }

    if (ahrs_home_is_set(ahrs)) {
        aplog_write_origin(APLOG_AHRS_HOME, ahrs_get_home2(ahrs));
    }
}

static void update_location_and_state()
{
    if (itc_copy_from_hub(ITC_ID(vehicle_origin), &origin_pos_g) != 0) {
        return;
    }

    if ((ahrs.origin_loc.lat != origin_pos_g.lat
        || ahrs.origin_loc.lng != origin_pos_g.lon)
        && origin_pos_g.valid_lpos) {
        location_from_lat_lon_alt(&ahrs.origin_loc, origin_pos_g.lat, origin_pos_g.lon, origin_pos_g.alt*100.0f, ALT_FRAME_ABSOLUTE);
        location_from_lat_lon_alt(&ahrs.curr_loc, origin_pos_g.lat, origin_pos_g.lon, origin_pos_g.alt*100.0f, ALT_FRAME_ABSOLUTE);
    }

    if (origin_pos_g.valid_lpos) {
        curr_loc_tmp = ahrs.origin_loc;

        location_offset(&curr_loc_tmp, local_position.x, local_position.y);
        location_set_alt_cm(&curr_loc_tmp, local_position.alt*100.0f, ALT_FRAME_ABSOLUTE);

        ahrs.curr_loc = curr_loc_tmp;
    }

    if (ahrs.origin_lat != origin_pos_g.lat) {
        ahrs.origin_lat = origin_pos_g.lat;
    }

    if (ahrs.origin_lng != origin_pos_g.lon) {
        ahrs.origin_lng = origin_pos_g.lon;
    }

    ahrs.valid_alt = origin_pos_g.valid_alt;
    ahrs.valid_hpos = origin_pos_g.valid_hpos;
    ahrs.valid_lpos = origin_pos_g.valid_lpos;
}

/// 
static void publish_vehicle_home()
{
    if (!ahrs.home_is_set) {
        return;
    }

    vehicle_home.timestamp_us = time_micros64();

    vehicle_home.valid_alt = ahrs.valid_alt;
    vehicle_home.valid_hpos = ahrs.valid_hpos;
    vehicle_home.valid_lpos = ahrs.valid_lpos;

    vehicle_home.alt = ahrs.home_loc.alt*0.01f;
    vehicle_home.z   = (ahrs.home_loc.alt - ahrs.origin_loc.alt) * 0.01f;

    vehicle_home.lat = ahrs.home_loc.lat;
    vehicle_home.lon = ahrs.home_loc.lng;

    Vector2f_t home_ne;
    location_get_vector_xy_from_origin_ne(&ahrs.home_loc, &home_ne);

    vehicle_home.x = home_ne.x * 0.01f;
    vehicle_home.y = home_ne.y * 0.01f;

    vehicle_home.yaw = ahrs.yaw;
    vehicle_home.mag_decl = ahrs.valid_hpos ? geo_get_declination((double)vehicle_home.lat*1e-7, (double)vehicle_home.lon*1e-7) : 0.0f;

    itc_publish(ITC_ID(vehicle_home), &vehicle_home);

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
    console_printf("publish_vehicle_home: lat:%d, lon:%d, alt: %f\n", vehicle_home.lat, vehicle_home.lon,
                                                   vehicle_home.alt);
#endif
}

static void update_accel_vibration_metrics(ahrs_view *ahrs)
{
    static Vector3f_t _acceleration_prev = {0};

    // Accel high frequency vibe = filtered length of (acceleration - acceleration_prev)
    ahrs->accel_vibration_metric = 0.99f * ahrs->accel_vibration_metric
                     + 0.01f * vec3_sub_length(&ahrs->acc, &_acceleration_prev);

    _acceleration_prev = ahrs->acc;
}

static void update_gyro_vibration_metrics(ahrs_view *ahrs)
{
    static Vector3f_t _angular_velocity_prev = {0};

    // Gyro high frequency vibe = filtered length of (angular_velocity - angular_velocity_prev)
    ahrs->gyro_vibration_metric = 0.99f * ahrs->gyro_vibration_metric
                    + 0.01f * vec3_sub_length(&ahrs->gyr, &_angular_velocity_prev);

    _angular_velocity_prev = ahrs->gyr;
}
/*------------------------------------test------------------------------------*/


